#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     liftUpSensor,   sensorTouch)
#pragma config(Sensor, S3,     liftDownSensor, sensorTouch)
#pragma config(Sensor, S4,     HTSPB,          sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     linearSlides,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     grabberMount,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     rearLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rearRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C4_1,    wristMain,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    grabberRight,         tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    grabberLeft,          tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    irPivot,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    rampLatch,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//B Brick

//Goes forward, stops, rotates, stops, drives onto peg rack platform (and stops) to
//stop one of the other robot's autonomous modes.

#include "JoystickDriver.c"

const int rampLatchOpen=0;
const int rampLatchClosed=175;
const int weightSensorFrontIndicator = 255;
const int weightSensorBackIndicator = 127;
const int weightSensorNoneIndicator = 0;

void initializeRobot ()
{
	/* If the wrist is moving too quickly try changing the divisor in the setWrist equation from 1000.0 to 10000.0;
	this will not make a huge difference, but it's worth a try. */
	servo [rampLatch] = rampLatchClosed;
	servo [irPivot] = weightSensorNoneIndicator;
}

void drive (int x1, int y1, int x2)
{
	motor[frontLeft] = (y1 + x2 + x1);
	motor[frontRight] = (y1 - x2 - x1);
	motor[rearLeft] = (y1 + x2 - x1);
	motor[rearRight] = (y1 - x2 + x1);
}

task main ()
{
	waitForStart();
	initializeRobot();
	drive (0, 60, 0);
	wait1Msec (3700);
	drive (0, 0, 0);
	wait1Msec(500);
	drive(0, 0, 50);
	wait1Msec (1000);
	drive (0, 0, 0);
	wait1Msec (1000);
	drive (0, 50, 0);
	wait1Msec(2300);
	drive (0, 0, 0);

}
